/*
**Author erow
**
*/
#include <iostream>
#include <sys/time.h>
#include <math.h>
#include <unistd.h>
#include <stdio.h>
#include "transitHub.h"
#include "Serial.h"

#include "robotModel.h"
#include "gaitStep.h"
transitHub* port;
void gaitTest()
{
    string help=\
"\
quit to quit program\n\
push to push data\n\
pop  to delete last data\n\
initgait \n\
getmpu   \n\
play \n\
load name\n\
help \n\
";
    cout<<help;
    GaitStep gait("recorder.gait",20);
    int data[20];string command;
    while(cin>>command)
    {
        float x,y,z,len;int s;
        //cin>>x>>y>>z;

        if(command=="quit")
            return;
        if(command=="initgait")
            {
            int tdata[20]={0};
            int n,v;
            while(1)
            {
            cin>>n>>v;
            tdata[n]=v;
            port->doLoopTx(tdata);
            if(n>=20)
                break;
            }
            }
        if(command=="push")
            gait.pushData(data);
        if(command=="pop")
            gait.popData();

        if(command=="getmpu")
        {
            Eigen::Map<Eigen::Matrix<double,1,3> > acc(port->p_acc);
            Eigen::Map<Eigen::Matrix<double,1,3> > gyp(port->p_gyp);
            cout<<"acc:"<<acc<<endl;
            cout<<"gyp:"<<gyp<<endl;
        }
        if(command=="dorx")
        {
        int t;cin>>t;
        while(t-- >=0)
        {
        port->doRx();
        }
        }
        if(command=="go")
        {
            int g=0;cin>>g;
            for(int i=0;i<min(g,gait.getLen());i++)
            {
                gait.getData(data,i);
                port->doLoopTx(data);
                usleep(20*1000);
            }
        }

        if(command=="?"||command=="help")
        {
            cout<<help;
        }
        if(command=="play")
        {
        double dt;
        cout<<"dt:";cin>>dt;
            for(int i=0;i<gait.getLen();i++)
            {
                gait.getData(data,i);
                port->doLoopTx(data);
                usleep(dt*1000);
            }
        }
        if(command=="load")
        {
            string n;cin>>n;
            if(gait.loadFile(n))
                cout<<"load successfully!\n";
            else
                cout<<"wrong\n";
        }
    }
    gait.saveFile();

}
void P3Test()
{
        string help=\
"\
setleg    (double rotateX,double rotateY,double rotateZ,double len,int leg);  \n\
setparam  (double rotateX,double rotateY);   \n\
fixparam  (double rotateX,double rotateY);   \n\
getmpu   \n\
help \n\
";
    cout<<help;
    robotModel_threePoint r;
    int data[20]={0};string command;
     while(cin>>command)
    {
        float x,y,z,len;int s;
        if(command=="fixparam")
        {
            cin>>x>>y;
            r.fixParam(x,y);
            r.getData(data);
        }
        if(command=="setleg")
        {
            cin>>x>>y>>z>>len>>s;
            r.setLeg(x,y,z,len,s);
            r.getData(data);
        }
        if(command=="setparam")
        {
            cin>>x>>y;
            r.setParam(x,y);
            r.getData(data);
        }
        if(command=="getmpu")
        {
            Eigen::Map<Eigen::Matrix<double,1,3> > acc(port->p_acc);
            Eigen::Map<Eigen::Matrix<double,1,3> > gyp(port->p_gyp);
            cout<<"acc:"<<acc<<endl;
            cout<<"gyp:"<<gyp<<endl;
        }
        port->doLoopTx(data);
        cout<<"cm:"<<r.cal_referCentroid()<<endl;
    }

}
int main(int arg,char** args){

    //port:1,baud_rate:57600
   // cin>>port>>way;
   logger::m_level=DEBUG;
   port=transitHub::getInstance();
    int data[20]={0};
    while(!port->portOpen(1,57600)){
            cout<<"fail!\n";
    cout<<"(port way)//ttyS$port"<<endl;
    return 1;
    }
    float s=0;
    string help="\
    quit to quit program\n\
    gaitTest\n\
    P3Test\n\
    help \n\
    ";
    string cmd;
    cout<<help;

    while(1)
    {
        cin>>cmd;
        if(cmd=="gaitTest")
            gaitTest();
        if(cmd=="P3Test")
            P3Test();

    }


    return 0;
}
